/*********************************************************************************
   #     # #     #    ######  ######   #####     #     #  #####  #######  #####
   ##   ## #  #  #    #     # #     # #     #    ##   ## #     # #       #     #
   # # # # #  #  #    #     # #     # #          # # # #       # #       #
   #  #  # #  #  #    ######  ######  ######     #  #  #  #####  ######  ######
   #     # #  #  #    #   #   #       #     #    #     # #             # #     #
   #     # #  #  #    #    #  #       #     #    #     # #       #     # #     #
   #     #  ## ##     #     # #        #####     #     # #######  #####   #####
**********************************************************************************
01001101 01010111  01010010 01010000 00110110  01001101 00110010 00110101 00110110 
**********************************************************************************
**********************************************************************************
##################      RP6 ROBOT SYSTEM - RP6 CONTROL M256      #################
##################         RoboBridge Project Version 1.0        #################
**********************************************************************************
**********************************************************************************
*/

#ifdef __cplusplus
#error "C ONLY!"
#endif

#ifndef __STDC__
#error "C ONLY!"
#endif

#ifndef __AVR__
#error "AVR-GCC ONLY!"
#endif

#ifndef __AVR_ATmega2560__
#error "ATmega2560 ONLY!"
#endif


#include "RP6M256Lib.h" 
#include "RP6I2CmasterTWI.h"								// I2C Master Library
#include "RP6M256_I2CMasterLib.h"							// Base Remote Control
//#include "RP6DrivingLights.h"								// Driving Light Control
#include "LCD03.h"											// LCD03 Library
#include "CMPS03.h"											// CMPS03 Library
#include "SRF08.h"											// SRF08 Library
#include "PCCommand.h"										// PC Interface Library
#include "PCF8574.h"										// PCF8574 Library
#include "RP6DrivingLightsNew.h"							// Driving Light Control
//#include "SDCard.h"											// SD Card Library
#include "Utils.h"											// Utilities
#include "Math.h"											// Math Functions


/********************************************************************************/
//List of devices
srf08_t				*srf=NULL;								// SRF08
cmps03_t			*cmps=NULL;								// CMPS03
pcf8574_t			*pcfLeft=NULL;							// PCF8574 left
pcf8574_t			*pcfRight=NULL;							// PCF8574 right
drivingLights_t		*drivingLights=NULL;					// Driving lights

uint16_t updateInterval = 1000;
uint8_t heartbeatSent=false;
uint8_t connected=false;
uint8_t rp6LEDs=0;
uint8_t heartbeatReSent=false;
uint8_t servoPosChanged=false;
uint8_t servoPos=116;
uint8_t measureSRF=true;
/********************************************************************************/

#define BASE_ID 1
#define SRF_ID 2
#define CMPS_ID 3
#define LCD03_ID 4
#define M256_ID 7

#define I2C_RNCONTROL_ADR 0x12

#define BUMPERS_CHANGED 1
#define ACS_CHANGED 2
#define MOVEMENT_COMPLETE 3
#define SPEED 4
#define DISTANCE 5
#define LIGHT 6
#define DESIRED_SPEED 7
#define MOTOR_CURRENT 8
#define BATTERY 9
#define BASE_LEDs 10

#define M256_LEDs 1

uint8_t blinkLeft;
uint8_t blinkRight;

void task_LCDHeartbeat(void){
	if(getStopwatch1() > 500){
		static uint8_t heartbeat = false;
		if(heartbeat){
			LCD03_clearPos(1, 1, 1);
			heartbeat = false;
		}else{
			LCD03_setCursorPos(1, 1);
			LCD03_writeString_P("#"); 
			heartbeat = true;
		}
		setStopwatch1(0);
	}
}

void watchDogRequest(void){
	static uint8_t heartbeat2 = false;
	if(heartbeat2){
		LCD03_clearPos(1,2,1);
		heartbeat2 = false;
	}else{
		LCD03_setCursorPos(1,2);
		LCD03_writeString_P("#"); 
		heartbeat2 = true;
	}
}

/********************************************************************************/
void I2C_requestedDataReady(uint8_t dataRequestID){
	checkRP6Status(dataRequestID);
	if(srf->checkData(dataRequestID,srf)&&connected){
		sendCommand4(COMMAND_UPDATE_DEVICE_DATA, SRF_ID, getHighByte(srf->range), getLowByte(srf->range), srf->light);
		measureSRF=false;
		return;
	}		
	if(cmps->checkData(dataRequestID,cmps)&&connected){
		sendCommand3(COMMAND_UPDATE_DEVICE_DATA, CMPS_ID, getHighByte(cmps->direction), getLowByte(cmps->direction));
		return;
	}
}

void I2C_transmissionError(uint8_t errorState){
	writeString_P("\nI2C ERROR - TWI STATE: 0x");
	writeInteger(errorState, HEX);
	writeChar('\n');
}

/********************************************************************************/
uint8_t getInputLine_WIFI(void){ 
	if(getBufferLength_WIFI()){				
		return parseLine_WIFI(readChar_WIFI());
	}		
	return 0;
}

void processBaseCommand(command_t *command){
	if(command->params<2){
		writeString_P("No Base Command id...\r\n");
		return;
	}
	switch(command->params[1]){
		#define STOP 1
		#define MOVEATSPEED 2
		#define DRIVE_DISTANCE 3
		#define ROTATE 4
		#define SET_LEDS_BASE 5
		case STOP:{
			stop();
			break;
		}	
		case MOVEATSPEED:{
			if(command->params<4){
				writeString_P("No Speed specified...\r\n");
				return;
			}
			writeString_P("Base command MoveAtSpeed!\r\n");
			moveAtSpeed(command->params[2], command->params[3]);
			break;
		}case DRIVE_DISTANCE:{
			uint16_t distance=command->params[4];
			distance<<=8;
			distance+=command->params[5];
			move(command->params[2], command->params[3], DIST_CM(distance), false);
			break;
		}case ROTATE:{
			uint16_t angle=command->params[4];
			angle<<=8;
			angle+=command->params[5];
			rotate(command->params[2], command->params[3], angle, false);
			break;
		}case SET_LEDS_BASE:{
			rp6LEDs=command->params[2];
			setRP6LEDs(command->params[2]);
			break;
		}
		
		default:{
			writeString_P("Invalid Base Command id...\r\n");
		}
	}		
}

void onCommandReceived(command_t *command){
	switch(command->id){
		case COMMAND_ACK:{
			LCD03_showScreen4("####################","##     MASTER     ##", "##      HELLO     ##","####################");
			sendCommand1(COMMAND_ACK, command->id);
			break;
		}
		case COMMAND_LOGIN:{
			setLEDs(0b0011);
			heartbeatSent=false;
			sendCommand1(COMMAND_ACK, COMMAND_LOGIN);
			writeString_P("Got LOGIN!\r\n");
			LCD03_showScreen4("####################","##     MASTER     ##", "##       OK       ##","####################");
			setStopwatch3(0);
			connected=true;
			break;
		}
		case COMMAND_HELLO:{
			writeString_P("Got HELLO\r\n");
			setLEDs(0b0001);
			sendCommand2(COMMAND_ACK, COMMAND_HELLO, 42);
			break;
		}
		case COMMAND_ECHO:{
			sendCommand(COMMAND_ECHO);
			break;
		}
		case COMMAND_PING:{
			sendCommand(COMMAND_PING);
			break;
		}
		case COMMAND_HEARTBEAT:{
			heartbeatSent=false;
			heartbeatReSent=false;
			writeString_P("Heartbeat received!\r\n");
			setStopwatch3(0);
			break;
		}
		case COMMAND_UPDATE_DEVICE:{
			if(!connected){
				break;
			}
			if(command->params<1){
				writeString_P("No Device id...\r\n");
				break;
			}
			if(command->params[0]==BASE_ID){
				processBaseCommand(command);
				break;
			}else if(command->params[0]==4){
				if(command->params[1]==1&&command->str){
					LCD03_setCursorPos(command->params[2], command->params[1]);
					LCD03_writeString(str);
				}else if(command->params[1]==2){
					if(command->params[2]){
						LCD03_writeCommand(LCD03_BACKLIGHT_ON);
					}else{
						LCD03_writeCommand(LCD03_BACKLIGHT_OFF);
					}
				}				
			}else if(command->params[0]==5){
				if(command->params[1]==1){
					//Blink!
					if(command->params[2]==1){
						blinkLeft=true;
					}else if(command->params[2]==2){
						blinkRight=true;
					}else if(command->params[2]==3){
						blinkLeft=true;
						blinkRight=true;
					}else{
						blinkLeft=false;
						blinkRight=false;
					}
				}else if(command->params[1]==2){
					//Headlights
					if(command->params[2]==1){
						drivingLights->leftState.FRONTLIGHT_LEFT=false;
						drivingLights->rightState.FRONTLIGHT_RIGHT=false;
						drivingLights->leftState.BACKLIGHT_LEFT=false;
						drivingLights->rightState.BACKLIGHT_RIGHT=false;
					}else{
						drivingLights->leftState.FRONTLIGHT_LEFT=true;
						drivingLights->rightState.FRONTLIGHT_RIGHT=true;
						drivingLights->leftState.BACKLIGHT_LEFT=true;
						drivingLights->rightState.BACKLIGHT_RIGHT=true;
					}					
				}else if(command->params[1]==3){
					//Reverse lights
					if(command->params[2]==1){
						drivingLights->leftState.REVERSELIGHT_LEFT=false;
						drivingLights->rightState.REVERSELIGHT_RIGHT=false;
					}else{
						drivingLights->leftState.REVERSELIGHT_LEFT=true;
						drivingLights->rightState.REVERSELIGHT_RIGHT=true;
					}
				}										
			}else if(command->params[0]==6){
				if(command->params[1]==1){
					//RNCONTROL: Servo
					servoPosChanged=true;
					servoPos= command->params[2];
					setStopwatch5(0);
					startStopwatch5();
					I2CTWI_transmit3Bytes(I2C_RNCONTROL_ADR, 0,1, command->params[2]);
				}				
			}else if(command->params[0]==2){
				if(command->params[1]==1){
					measureSRF=true;	
				}					
			}							
			break;
		}
		case COMMAND_SET_SENSOR_INTERVAL:{
			updateInterval=command->params[0]<<8;
			updateInterval+=command->params[1];
			sendCommand1(COMMAND_ACK, COMMAND_SET_SENSOR_INTERVAL);
			break;
		}
		default:{
			
		}
	}
}

void motionStateChanged(void){
	sendCommand3(COMMAND_UPDATE_DEVICE_DATA, BASE_ID, MOVEMENT_COMPLETE, isMovementComplete());
}

void bumpersStateChanged(void){
	sendCommand4(COMMAND_UPDATE_DEVICE_DATA, BASE_ID, BUMPERS_CHANGED, bumper_left, bumper_right);
}

void acsStateChanged(void){
	sendCommand4(COMMAND_UPDATE_DEVICE_DATA, BASE_ID, ACS_CHANGED, isObstacleLeft(), isObstacleRight());
}

/********************************************************************************/
int os_main main(void){
	//Init the M256
	initRP6M256(); 

	WDT_setRequestHandler(watchDogRequest); 
	
	//Setup I2C
	I2CTWI_initMaster(100);  
	I2CTWI_setRequestedDataReadyHandler(I2C_requestedDataReady);
	I2CTWI_setTransmissionErrorHandler(I2C_transmissionError);
	MOTIONCONTROL_setStateChangedHandler(motionStateChanged);
	
	BUMPERS_setStateChangedHandler(bumpersStateChanged);
	ACS_setStateChangedHandler(acsStateChanged);
	//Setup Command Interface
	setCommandReceptionHandler(onCommandReceived);
	
	//Display info text
	setLEDs(0b1111);
	LCD03_writeCommand(LCD03_BACKLIGHT_ON);
	LCD03_writeCommand(LCD03_HIDE_CURSOR);
	LCD03_showScreen4("####################","####################", "####################","####################");
	mSleep(500);
	LCD03_showScreen4("####################","##RP6 CONTROL M256##", "#RoboBridge Project#","####################");
	mSleep(500);
	setLEDs(0b0000);
	//Base initialization
	// Setup ACS power:
	I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_ACS_POWER, ACS_PWR_OFF);
	// Enable Watchdog for Interrupt requests:
	I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT, true);
	// Enable timed watchdog requests:
	I2CTWI_transmit3Bytes(I2C_RP6_BASE_ADR, 0, CMD_SET_WDT_RQ, true);

	startStopwatch8();

	//Sensor setup
	MAKE_SRF08(srf,0xE6,&getStopwatch8());
	MAKE_CMPS03(cmps,0xC0);
	MAKE_PCF8574(pcfLeft,0x40);
	MAKE_PCF8574(pcfRight,0x4E);
	
	drivingLights = createDrivingLights(pcfLeft, pcfRight);
	initializeDrivingLights(drivingLights);
	
	startStopwatch1();
	startStopwatch2();
	startStopwatch3();
	startStopwatch4();
	
	// Make sure WLAN Module Packet size / buffer size and flush timer is OK
	enter_cmd_mode_WIFI();   
	issueCMD_WIFI("set comm size 1024","AOK"); 
	issueCMD_WIFI("set comm time 10","AOK"); 
	leave_cmd_mode_WIFI();

	for (;;){
		if(getInputLine_WIFI()){
			//New line available!
			if(strlen(receiveBuffer_WIFI)>=3){
				parseCommand(receiveBuffer_WIFI);
			}
			
			//clearReceptionBuffer_WIFI();
		}
		
		if(connected){
			if(measureSRF){
				//Start a SRF08 measurement if possible
				srf->measure(srf);
			}
			
			//Start a CMPS03 measurement if possible
			cmps->measure(cmps);
			if(getStopwatch3()>=1000){
				if(!heartbeatSent){
					sendCommand(COMMAND_HEARTBEAT);
					heartbeatSent=true;
					setStopwatch3(0);
					writeString_P("Heartbeat sent!\r\n");
				}else if(getStopwatch3()>=2000){
					if(!heartbeatReSent){
						heartbeatReSent=true;
						writeString_P("Heartbeat re-sent!\r\n");
						sendCommand(COMMAND_HEARTBEAT);
					}else if(heartbeatReSent&&getStopwatch3()>2500){
						writeString_P("Heartbeat re-sent and no answer!\r\n");
						LCD03_showScreen4("####################","##     MASTER     ##", "##     TIMEOUT    ##","####################");
						connected=false;
						drivingLights->leftState.byte=255;
						drivingLights->rightState.byte=255;
						setLEDs(0);
						setRP6LEDs(0);
						rp6LEDs=0;
						updateInterval=1000;
						setStopwatch3(0);
						stop();
					}
				}
			}
		}else{
			if(getStopwatch3()>=1000){
				writeString_P("No connection\r\n");
				LCD03_showScreen4("####################","##     MASTER     ##", "##       N/A      ##","####################");
				blinkLeft=true;
				blinkRight=true;
				setStopwatch3(0);
				setLEDs(0b0000);
			}			
		}
		static uint8_t on=false;
		if(!on&&getStopwatch4()>=666){
			on=true;
			if(blinkLeft){
				drivingLights->leftState.BLINKER_FRONT_LEFT=false;
				drivingLights->leftState.BLINKER_BACK_LEFT=false;
			}else{
				drivingLights->leftState.BLINKER_FRONT_LEFT=true;
				drivingLights->leftState.BLINKER_BACK_LEFT=true;
			}
			if(blinkRight){
				drivingLights->rightState.BLINKER_FRONT_RIGHT=false;
				drivingLights->rightState.BLINKER_BACK_RIGHT=false;
			}else{
				drivingLights->rightState.BLINKER_FRONT_RIGHT=true;
				drivingLights->rightState.BLINKER_BACK_RIGHT=true;
			}
		}else if(on&&getStopwatch4()>=1500){
			on=false;
			drivingLights->rightState.BLINKER_FRONT_RIGHT=true;
			drivingLights->rightState.BLINKER_BACK_RIGHT=true;
			drivingLights->leftState.BLINKER_FRONT_LEFT=true;
			drivingLights->leftState.BLINKER_BACK_LEFT=true;
			setStopwatch4(0);
		}
		
		drivingLights->update(drivingLights);
		if(servoPosChanged&&getStopwatch5()>20){
			servoPosChanged=false;
			sendCommand3(COMMAND_UPDATE_DEVICE_DATA, 6, 1, servoPos);
			setStopwatch5(0);
			stopStopwatch5();
			measureSRF=true;
			srf->measure(srf);
		}
		if(connected&&getStopwatch2()>=updateInterval){
			getAllSensors();
			sendCommand3(COMMAND_UPDATE_DEVICE_DATA, M256_ID, 2, getPressedKeyNumber());
			sendCommand3(COMMAND_UPDATE_DEVICE_DATA, BASE_ID, MOVEMENT_COMPLETE, isMovementComplete());
			sendCommand4(COMMAND_UPDATE_DEVICE_DATA, BASE_ID, SPEED, getLeftSpeed(), getRightSpeed());
			sendCommand6(COMMAND_UPDATE_DEVICE_DATA, BASE_ID, DISTANCE, getHighByte(getLeftDistance()), getLowByte(getLeftDistance()), getHighByte(getRightDistance()), getLowByte(getRightDistance()));
			sendCommand6(COMMAND_UPDATE_DEVICE_DATA, BASE_ID, LIGHT,  getHighByte(adcLSL), getLowByte(adcLSL), getHighByte(adcLSR), getLowByte(adcLSR));
			sendCommand4(COMMAND_UPDATE_DEVICE_DATA, BASE_ID, DESIRED_SPEED, mleft_des_speed, mright_des_speed);
			sendCommand6(COMMAND_UPDATE_DEVICE_DATA, BASE_ID, MOTOR_CURRENT,  getHighByte(adcMotorCurrentLeft), getLowByte(adcMotorCurrentLeft), getHighByte(adcMotorCurrentRight), getLowByte(adcMotorCurrentRight));
			sendCommand4(COMMAND_UPDATE_DEVICE_DATA, BASE_ID, BATTERY, getHighByte(adcBat), getLowByte(adcBat));
			sendCommand3(COMMAND_UPDATE_DEVICE_DATA, BASE_ID, BASE_LEDs, rp6LEDs);
			sendCommand3(COMMAND_UPDATE_DEVICE_DATA, M256_ID, M256_LEDs,PORTC&0xC3);
			setStopwatch2(0);
		}
		
		task_LCDHeartbeat();
		task_checkINT();
	    task_I2CTWI();
	}
	return 0;
}